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非 线 性 滤波 在 SINS 中 的 应 用 
aryr, X, HA 
(中 国 科 学 院 大 学 国家 天 文 台北 京 ) 
摘要 : 针对 载体 的 姿态 和 角速度 估计 问题 ,分 别 给 出 了 基于 卡尔 曼 滤波 和 粒子 滤波 的 估计 算法 。 对 于 欧 拉 
角 带 来 的 奇异 问题 ， 采 用 四 元 数 描述 载体 的 姿态 角 。 通 过 对 mpu9250 模块 (陀螺 仪 、 加 速度 计 和 磁力 计 》 
进行 数据 采集 ， 并 运用 实验 数据 和 仿真 运算 ， 验 证 了 四 元 数 卡尔 曼 滤 波 和 粒子 滤波 算法 的 可 行 性 。 静 态 实 
验 和 动态 仿真 的 计算 结果 表明 ， 在 使 用 MEMS 器 件 测量 载体 姿态 时 ， 粒 子 滤波 和 卡尔 曼 滤 波 算法 得 到 的 误 
差 均 值 相当 ， 但 粒子 滤波 的 标准 差 相 对 较 小 。 
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随 着 微小 型 飞行 器 和 机 器 人 等 的 姿态 估计 常常 采用 滤波 器 对 来 自 微机 械 MEMS) 惯性 测 
量 单元 (MU) 的 多 传感器 数据 进行 融合 得 到 ， 常 用 的 MEMS 器 件 测量 器 件 包 括 三 轴 陀 螺 仪 、 
三 轴 加 速度 计 和 三 轴 磁 力 计 本 文采 用 四 元 数 进行 姿态 解 算 。 文 [1] 进 行 了 四 元 数 卡 尔 曼 滤 波 
方程 的 建立 和 方程 , 文 [2] 对 文 [1] 提 出 的 四 元 数 卡 尔 曼 滤 波 进行 性 能 分 析 。 同 时 很 多 人 也 进 
行 了 粒子 滤波 在 姿态 信 计 中 的 研究 。 文 [和 和 文 [9] 通过 对 四 元 数 的 分 析 ， 设 计 了 四 元 数 粒 
子 滤波 算法 求 取 姿态 。 本 文通 过 对 微机 械 器 件数 据 采集 和 处 理 , 设计 非 线性 四 元 数 卡尔 曼 渡 
波 和 粒子 滤波 ， 并 对 其 姿态 解 算 结果 进行 进一步 分 析 。 
1 四 元 数 简介 

载体 上 的 微机 械 惯 性 测量 器 件 的 输出 转换 成 载体 的 姿态 , 即 载体 自身 坐标 系 (xp, p, Zp) 


ni 


相对 于 导航 坐标 系 Qs Yao za 的 角 位 置 ， 写 成 矩阵 形式 如 下 : 
Xp Xn 
p ace ra| q 
Zp Zn 


' 欧 拉 角 是 载体 的 3 个 姿态 角 ， 即 俯仰 角 (pitch)、 横 滚 角 (rol1) 和 偏 航 角 (yaw)。 
根据 欧 拉 旋转 定律 , 可 以 通过 3 次 旋转 使 得 载体 坐标 系 与 导航 坐标 系 重合 , 每 一 次 旋转 都 以 
导航 坐标 系 的 一 个 坐标 轴 来 转动 , 转 过 的 角度 就 是 欧 拉 角 , 每 次 旋转 后 坐标 关系 可 由 一 旋转 
和 矩阵 表示 ， 即 方向 余弦 和 矩阵，(1) 式 采 用 zy 的 旋转 顺序 依次 旋转 : 


Az y-x (9, 8, 0)-A; (6)A, (9)A, (v)- 


C0SO * cosg cos * sing —sin0ü 
sin + sin0 * cos —cosÜ *sinp  sinÜ * sin0 * sing + cosÓ *cosp sin *cosü|, (2) 
COSÓ * sin * cosq + sinÜ * sinp  cosQ * sing * sing — sinÜ *cosp  cosQ * cos 


Jp, 9, 29，0 分 别 代 表 偏 航 角 、 横 滚 角 、 俯 仰角 。 为 了 避免 欢 拉 角 在 表示 姿态 时 可 能 出 现 
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的 奇异 问题 ， 四 元 数 在 载体 的 姿态 表示 方面 有 广泛 的 应 用 。 设 计 载 体 姿 态 四 元 数 为 


q = (qo,91,92,93) = qo + d1*i + q2 * j + q3 * k, 


q6 *qi—d2—4$ 2(qiqz +qoq3)  2(q1q3 — 4092) 


A(q)h =| 2(qiqz —dod43) q-q? +q3- q} 2(q293 + qoq91) | 
2(q193 * qod2)  2(q2q43— qod1) — d6 — qi -q2 * di 
简 记 为 : 
Tii Ti; Tis 
A(q)? =|Ta T7; 723|. 
Tai Ta; T33 
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(3) 


导航 坐标 系 与 载体 坐标 系 之 间 的 坐标 变换 可 以 用 方向 余弦 矩阵 表示 ， 其 四 元 数 形 式 为 


(4) 


(5) 


导航 坐标 系 到 载体 坐标 系 的 旋转 变换 过 程 中 坐标 系 始终 保持 直角 坐标 系 ， 所 以 4(9)3 为 正 交 


BIRR, BACACAR = (Aq) 。 从 而 载体 的 姿态 角 为 
0 = —arcsinT4s 


T 
Ø = arctan -2 
Tuc 


9 = arctan m 
四 元 数 的 微分 方程 如 下 : 


. 1 
d —5q'0, 


(65 


S 


其 中 , a 为 描述 载体 转动 的 四 元 数 ，o 为 载体 相对 导航 参考 坐标 系 的 角速度 ， 也 表示 为 


数 的 形式 : 


o-0-toy:itoy:j tok, 
do 0 一 wx Oy -—9;]rqo 
d| 1i|e 0 wz —Oy||qi 
d;|| 2|0y 一 oz 0 wx ||d42| 
d Wz Oy  —w 0 


a 1 
q— 5*0, *q. 


(8) 


(9) 


(105 


次 
2 四 元 数 卡 尔 曼 滤波 (EKF) 
在 四 元 数 和 非 线性 滤波 算法 的 结合 中 , 最 党 
中 使 用 文 [1] 的 四 元 数 卡 尔 曼 滤波 ， 渡 波 过 程 简介 如 下 : 
(1) 滤波 器 初始 化 


UH 


通过 三 轴 陀 螺 仪 测量 的 3 个 轴 的 角速度 就 可 以 实现 实时 更 新 四 元 数 的 值 , 进而 更 新 姿态 角 获 


的 算法 就 是 基于 四 元 数 卡 尔 曼 滤波 ， 本文 


为 8 和 RR 选取 初始 值 , 可 以 通过 对 静态 下 三 轴 陀 螺 仪 、 三 轴 加 速度 计 和 三 轴 磁 力 计 进 行 


测量 , 计算 出 各 个 轴 的 方差 作为 滤波 器 数据 误差 ; 并 通过 初始 加 速度 计 测量 出 俯仰 


和 横 深 
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Pojo = 14 作 为 系统 初始 噪声 。 
2) 状态 方程 传播 


1 
Pea = Viene Desk +7 Mig )la — My] Qs- 


k+1/k ~ 


(3) 量 测 方程 更 新 


， 并 用 磁力 计 测量 出 方位 角 作为 初始 姿态 ， 


QL = H(wh,), 
Oy ay = exp G * p *^ t), 


qk+1/k = Pk+1/k * dk 


Myjy = qk/klk/k + Per 


Mya = eakdesik + BS ane 


把 初始 姿态 角 转 变 为 初始 


y 元 数 qoyo， 选择 


(11) 


(12) 


(13) 
(14) 


(15) 


首先 通过 预测 得 到 的 qxpyyx 应 用 (4) 式 计算 出 观测 方程 中 的 系数 矩阵 H(qk+a). 


(16) 


Bk+1 = Hia), (17) 


有 HA = Rea [tr Misa) — Mia — BisaMiiajkBisa]: (18) 
Sea = H(qeay) * Dank * Hen): + Praze (19) 

Kx+1 = Ban * H(qrrvx) Scd (20) 

Qa ks = dieafk + Kei Diss — H (arrije) * nisab (21) 
Phan = (1 x Kei H(qeaye) Ban (22) 


3 ”粒子 滤波 


粒子 滤波 是 通过 寻找 一 组 在 状态 空间 中 传播 的 随机 样本 近似 表示 状态 变量 , 用 样本 均值 


必 蔡 积分 运算 ， 进 而 获得 系统 最 小 方差 的 过 程 ， 这 些 村 
下 : 对 于 平稳 的 随机 过 程 ， 假 设 K-1 时 刻 系 统 的 后 验 概率 为 p(xk-1|2zx-l) ,依据 一 定 原则 选 
取 个 随机 样本 ，Kk 时 刻 获 得 测量 更 新 后 ， 经 过 状态 和 时 间 过 程 ， 个 粒子 的 后 验 概率 密度 


可 近似 为 p(xx |zx)， 随 着 粒子 数 
函数 ， 从 而 粒子 滤波 达到 最 优 贝 叶 
假设 非 线 性 动态 离散 系统 为 


本 被 称 为 粒子 。 采 


数学 语言 描述 如 


的 增加 ， 粒 子 的 概率 密度 函数 逐渐 逼近 状态 的 概率 密度 


i 售 计 的 效果 中 


Xk+1 7 nc Wy) 


Zk = hy (xy; vy) 


HK, xy € R" 为 k 时 刻 的 n Ze IR ES Ze € RT 73 m 维 观测 向 量 ，wi 和 vx 分 别 为 过 程 噪 


二 
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和 量 测 噪声 。 


粒子 滤波 算法 步骤 如 下 : 


(1) ”初始 化 : k = 0 时 ， 产 生 服从 初始 概率 密度 p(xo) 的 W 个 粒子 点 xD 由 一 工 …mi 


(2) ”通过 重要 性 采样 更 新 样本 粒子 状态 ， 


za (nh wa) ， i 
Kea) iN ， am 


(3) ”计算 更 新 后 粒子 的 权 值 


w® =w® p(z pe Ge pica) (27) 


Pei 
(4) ” 归 一 化 粒子 权 值 
wO = we /Twe. (28) 
(5) 重 采 样 : 根据 各 自 归 一 化 权 值 w 的 大 小 复制 / 含 弃 样 本 ， 得 到 N 个 近似 服从 
P(E) jaa) attt. bw = 二 EN 


(6) ”输出 结果 : 算法 的 输出 是 粒子 集 {xQ3,i = 1 N], 用 它 可 以 表示 后 验 概率 和 函数 
g(xoxx) 的 期 望 : 
Polzer) = X xÉ, (dX); (29) 


E(g(o4) = 7 X gr). (30) 

CD). Sk=k+1, 重复 以 上 步骤 。 
4 数据 实验 

实验 通过 选择 传感器 模块 mpu9250 模块 , 它 包 含 三 轴 陀 螺 仪 、 三 轴 加 速度 计 和 三 轴 磁力 
ib, 能 够 通过 自身 所 有 的 低 通 滤波 器 和 A/D 变换 模块 直接 输出 九 轴 传感器 数据 . 实验 通过 将 
mpu6050 固定 在 转台 上 测量 ， 在 安装 过 程 中 远离 环境 磁场 的 干扰 。 采 用 单片机 读 取 mpu9250 
测量 数据 后 通过 120 串口 传输 给 电脑 进行 matlab 处 理 ， 以 检验 算法 的 可 用 性 和 精度 。 
CD 首先 将 传感器 模块 mpu9250 在 静止 状态 下 测量 输出 数据 数据 , 采样 速率 为 100Hz, 采样 
分 钟 ， 分 析 静 态 情况 下 的 九 轴 和 输出 数据 。 陀 螺 仪 三 轴 静 态 数据 如 图 1。 
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图 1 陀螺 仪 三 轴 静 态 输出 数据 


Fig.1Three-axis gyroscope static output data 


图 1 是 1 分 钟 采 集 的 陀螺 仪 三 轴 数 据 , 自 上 而 下 依次 是 下 x 轴 、 轴 和 z 轴 , 运用 MATLAB 
软件 分 析 其 x 轴 均 值 和 方差 分 别 为 0.0205 C) /s 和 1.238e(-4) (9) /s; y AAMER 
方差 分 别 为 :一 0.0048 (9) /s 和 1.57e(—4) C) /s;z 轴 的 均值 和 方差 为 :0.0154 (9) 


/s 和 2.3699e(—4) (°) /s; 
同样 的 方法 处 理 三 轴 加 速度 计 和 三 轴 陀 螺 仪 的 原始 数据 ， 结 果 见 表 1。 
del 加 速度 计 和 陀螺 仪 输出 数据 的 均值 和 方差 


Table 1.mean and variance of accelerometer and gyro output data 


三 轴 加 速度 计 〈g) 三 轴 磁 力 计 (uT) 
了 轴 Ys Züh 了 轴 Ys Zl 
均值 0.4 -0.5 9.4 3.6 4.5 24 
方差 0.0027 | 0.0033 | 0.0031 0. 02 0. 096 0. 01 
通过 以 上 测量 , 加 速度 计 在 水 平 位 置 xY 轴 和 了 轴 均 值 不 为 零 , 所 以 在 做 姿态 解 算 时 引入 


均值 误差 提高 测量 精度 。 
(2) ”通过 上 面 静态 数据 的 读 取 ， 将 各 个 传感器 的 方差 作为 四 元 数 卡 尔 曼 渡 波 和 粒子 滤波 
程序 的 方差 参考 ， 静 止 状态 下 四 元 数 卡尔 曼 滤波 和 粒子 滤波 的 姿态 角 结 算 误差 如 图 


2、 图 3。 
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图 2 四 元 数 卡尔 曼 滤 波 静 态 三 轴 姿 态 角 误差 
Fig 2. Quaternion Kalman filter static triaxial attitude angle error 
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图 3 ”粒子 滤波 静态 三 轴 姿 态 角 误差 


Fig 3.particle filter static triaxial attitude angle error 
静止 状态 下 四 元 数 卡 尔 曼 滤波 和 粒子 滤波 三 轴 姿 态 角 均值 和 误差 如 表 2。 
表 2 静止 状态 两 种 算法 的 均值 和 方差 


Table 2. Mean and variance of the two algorithms for the quiescent state 


[元 数 卡 尔 曼 粒子 滤波 
xh Y Zh Xt 了 轴 2Z 轴 
均值 0. 002 0. 0036 0.0027 | -0.0337 | 0.0369 0. 0016 
方差 0. 0106 0. 0087 0. 0101 0. 0143 0. 0024 0. 0045 


(3) 


通过 对 静态 数据 的 分 析 可 以 看 出 ， 两 种 算法 在 均值 改善 上 基本 差不多 ， 但 是 粒子 滤波 
比 四 元 数 卡 尔 曼 滤波 在 方差 上 有 所 改善 ， 即 粒子 滤波 在 静态 测量 相对 稳定 。 

由 于 没有 转台 等 实验 设备 ， 通 过 对 静态 数据 上 添加 一 个 稳定 的 姿态 仿真 轨迹 计算 
元 数 卡尔 曼 滤 波 和 粒子 滤波 在 动态 情况 下 的 结算 结果 。 通 过 对 三 轴 姿 态 角 分 别 添加 


zi 
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5 


角 速 率 为 2，3，5 (D /s, 和 幅度 为 5 仿真 ， 得 到 如 图 4 的 仿真 结果 。 


P Mitth iHi M 
-0.5 L L L En = à 
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0.5 T r 
o Mei Poli ayha phy aAA 
0.5 ? s 


40 50 60 
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0.5 i i 
o aahua AI gl TD ln NAAA 
is^ 10 20 30 40 50 60 


图 4 两 种 算法 姿态 角 误 差 比较 


Fig 4. Comparison of Attitude Angle Errors of Two Algorithms 
表 3 仿真 状态 两 种 算法 的 均值 和 方差 


Table 3 The mean and variance of the two algorithms for Simulation states 


四 元 数 卡尔 曼 粒子 滤波 
xh 了 轴 Zh X Yt 2Z 轴 
均值 0. 0301 0. 0201 0.0254 | -0.0347 0. 0180 0. 0204 
方差 0. 014 0. 0095 0. 0134 0. 0161 0. 0050 0. 0085 


通过 对 动态 的 仿真 实验 分 析 可 以 看 出 ， 两 种 算法 在 均值 改善 上 和 静态 数据 基本 相同 ， 
但 是 粒子 滤波 比 四 元 数 卡 尔 曼 滤波 在 方差 上 有 所 改善 ， 即 动态 情况 下 粒子 滤波 相对 
元 数 稳定 。 


zi 


结论 
本 文通 过 对 四 元 数 卡 尔 曼 滤 波 和 粒子 滤波 进行 简单 介绍 , 并 通过 对 mpu9250 进行 数据 采 


集 和 仿真 实验 , 验证 粒子 滤波 和 四 元 数 卡 尔 曼 滤 波 的 可 行 性 , 通过 以 误差 均值 和 标准 差 为 检 


验 标准 


， 验 证 了 粒子 滤波 相对 于 四 元 数 卡 尔 曼 滤波 提高 了 标准 差 。 
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Application of nonlinear filtering in SINS 
YU Shao-shao, PEI Jun, HU Chao 

(University of Chinese Academy of Science, National Astronomical Observatory, Bei jing) 
Abstract: Two algorithms are presented to solve the carrier attitude and rate 
estimation problem. One is based on Kalman filter and the other is based on Particle filter. In 
order to avoid the singular problem from Euler angle,we chooses quaternion to describe the 
attitude angle. We carried out the data acquisition of gyroscope, accelerometer and magnetometer 
of mpu9250 module. By compared with the experimental data and simulation, we dicusse the 
feasibility of quaternion Kalman filter and particle filter algorithm. The results of static and 
dynamic simulation show that the error of the particle filter and the Kalman filter is similar when 
the carrier attitude is measured by the MEMS device, and the standard deviation of the particle 


filter is relatively small. 


Key words: Quaternion; Kalman filter; Particle filter; Attitude estimation 
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